#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Sensor, in2,    armPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    autonPot,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  area,           sensorDigitalIn)
#pragma config(Sensor, dgtl2,  color,          sensorDigitalIn)
#pragma config(Sensor, dgtl3,  button,         sensorTouch)
#pragma config(Sensor, I2C_1,  encoderRight,   sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  encoderLeft,    sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           backleft,      tmotorVex393, openLoop, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port2,           left,          tmotorVex393, openLoop)
#pragma config(Motor,  port3,           armright1,     tmotorVex269, openLoop)
#pragma config(Motor,  port4,           armleft1,      tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port5,           spin,          tmotorVex269, openLoop)
#pragma config(Motor,  port6,           spin2,         tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port7,           armleft2,      tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port8,           armright2,     tmotorVex269, openLoop)
#pragma config(Motor,  port9,           right,         tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          backright,     tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "PIDController.c"
/****************************************************************************************DRIVING FUNCTIONS*****************************************************************************************/
int leftDrivePower, rightDrivePower, x, y, rampLimit = 2, leftDrivePrevious, rightDrivePrevious;//drivetrain variables
int gyroDrift=0;
int offset=0;
int potCutoff=2000;
bool offense=false;


// LDR November 11, 2011 -- armPot: armFloor = 1975, armHighPlace = 3730, armMax =
int armPower, spinPower, armPosition, armTolerance, initialArmPosition, armOffset;//arm tolerance to be set
int armFloor = 1875, armLowScore = 2490, armMidScore = 2900, armHighScore = 3670, armMax = 3845, armSetPoint, armState = 0;//arm positions. Place is position above goal, Score is on goal for tubes, floor is start position
int armInput, spinInput, armPreviousInput, armRampLimit = 2, rampSpinLimit = 0.2;//arm variables used in competitions code. Placed here to keep them with the rest of the arm variables
double revDistance=12.566370614359172953850573533118; //distance traveled in one revoluton, inches
bool armPositionReached,  slowSpin = false;
int isolation;

int fmin(int v1,int v2){
  if(v1>v2)return v2;
  else return v1;
}
int fmax(int v1,int v2){
  if(v1>v2)return v1;
  else return v2;
}


void resetDriveVariables(){
  leftDrivePower = rightDrivePower = x = y = leftDrivePrevious = rightDrivePrevious = 0;
}
void setDriveMotors(){
  motor[left] = leftDrivePower;
  motor[backleft] = leftDrivePower;
  motor[right] = rightDrivePower;
  motor[backright] = rightDrivePower;
}

void stopDrive(){
  rightDrivePower = leftDrivePower = 0;
  setDriveMotors();
}

void driveArcade(int power, int turn, bool square, bool ramp){
  if(square)
  {
    if(power != 0)
    {
      y = ((power*power)/127)*(power/abs(power));
    }
    else
    {
      y = 0;
    }

    if(turn!=0)
    {
      x = ((turn*turn)/127)*(turn/abs(turn));
    }
    else
    {
      x = 0;
    }
  }
  else
  {
    y = power;
    x = turn;
  }

  leftDrivePower = y+x;
  rightDrivePower = y-x;

  if(ramp == true)
  {
    if(abs(rightDrivePower) > abs(rightDrivePrevious) + rampLimit)
    {
      if(rightDrivePower > (rightDrivePower + rampLimit))
      {
        rightDrivePower = rightDrivePrevious + rampLimit;
      }
      else
      {
        rightDrivePower = rightDrivePrevious - rampLimit;
      }
    }

    if(abs(leftDrivePower) > abs(leftDrivePrevious) + rampLimit)
    {
      if(leftDrivePower > (leftDrivePower + rampLimit))
      {
        leftDrivePower = leftDrivePrevious + rampLimit;
      }
      else
      {
        leftDrivePower = leftDrivePrevious - rampLimit;
      }
    }
  }

  setDriveMotors();

  leftDrivePrevious = leftDrivePower;
  rightDrivePrevious = rightDrivePower;
}// end drivearcade

void driveDistance(int dist, int power){//just take the stuff from this and use it in autonomous
  int offset=SensorValue[encoderLeft];
  while((SensorValue[encoderLeft]-offset)*revDistance < dist){
    driveArcade(power,0,false,false);
  }
}

void turn(int angle, int power, bool left){
  time1[T2]=0;
  SensorValue[gyro]=0;
  float gyroError=0;
  int currentAngle=SensorValue[gyro];
  int OriginalTargetAngle=angle*10; //gyros measure in tenths of a degree
  int targetAngle=OriginalTargetAngle;
  if(!left){
    OriginalTargetAngle=-OriginalTargetAngle;
    power=-power;
  }

  while(abs(currentAngle)<abs(targetAngle)){
      gyroError=time1[T2]/1000*gyroDrift;
      targetAngle=OriginalTargetAngle+gyroError;
      driveArcade(0,-power,false,false);//right is positive, according to driveArcade, but negative according to gyro.
      currentAngle=SensorValue[gyro];
  }
  driveArcade(0,power,false,false);//brake
  driveArcade(0,0,false,false);
}//end turn

void driveStraight(int power, int distance){
  time1[T2]=0;
  SensorValue[gyro]=0;
  float gyroError=0;
  float correction=0;

  while(false){
      gyroError=(time1[T2])/1000*gyroDrift;
      correction=(SensorValue[gyro]-gyroError)/3*2;
      driveArcade(power,correction,false,false);
  }
  driveArcade(0,0,false,false);
}//end driveStraight

/*******************************************************************************************Arm & Spinner Code****************************************************************************************/

/*********PID Variables****************/
/**************************************/
PIDController Arm;

int ArmSetPoint;
int ArmKp = 10,ArmKi = 0, ArmKd = 0;

/**************************************/
/**************************************/

void setArmPids(short kp,short ki,short kd){
  ArmKp = kp;
  ArmKi = ki;
  ArmKd = kd;
  setPIDs(Arm, ArmKp, ArmKi, ArmKd);
}



void armMove(int aPower)
{
  armPower = aPower;
  motor[armleft1] = motor[armleft2] = motor[armright1] = motor[armright2] = armPower;
}

void spinMove(int sPower)
{
  spinPower = sPower;
  motor[spin] = motor[spin2] = spinPower;
}




/*****************************************************************************************Competition Code*****************************************************************************************/
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
  //LDR Intialize Gyro on in2
//SensorType[in2] = sensorNone;
//wait1Msec(1000);
//SensorType[in2] = sensorGyro; //+ is ccw, - is cw
//wait1Msec(2000);
//calculate gyroscope drift
//SensorValue[gyro]=0;
//ClearTimer(T2);//we will use T2 for the gyroscope
//int original=SensorValue[gyro];
//wait1Msec(1000);
//int final=SensorValue[gyro];
//gyroDrift=(final-original);
//ClearTimer(T2);
//SensorValue[gyro]=0;
SensorValue[encoderRight]=SensorValue[encoderLeft]=0;
offset=0;
if(SensorValue[autonPot]>potCutoff)
  offense=true;
else
  offense=false;

if(SensorValue[armPot]<1000){
 armFloor=90;
 armLowScore=110;
 armMidScore=120;
 armHighScore=135;
}


  //init(Arm,1);
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// you must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{


  bool interaction = (SensorValue[area] == 1); //user input switch to set if interaction zone or not (isolation)
  bool red = (SensorValue[color] == 1); //user input switch to set if on red team or not (blue)

  if (interaction) //if in interaction
  {
    if (red || !red) //if on red or blue team
    {
      goto HELL;
	    HELL:
		 time1[T1] = 0;
		  while(time1[T1] < 1300) //flip out rotors and intake pre-load
			    spinMove(128);
			spinMove(0);

			time1[T1] = 0;
		  while(time1[T1] <1500)  // raise arm to high goal level
			    armMove(100);

		  time1[T1] = 0;
		  while (time1[T1] < 2000) //hold up arm to allow for loading
		      armMove(35);
		  while(SensorValue(button)==0){//wait for button press, which will change the value to 1
		 }

		  time1[T1] = 0;
		  while (time1[T1] < 4500) //Drive forwards to goal, (while holding up arm)
		     driveArcade(40,0,false,false);

		  driveArcade(20,0,false,false);
		  time1[T1] = 0;
		  while (time1[T1] < 3000) //Release load and score!! (while holding up arm)
			  spinMove(-80);

		  armMove(0);
		  spinMove(0);
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//Drive home and lower arm
		  while (time1[T1]<1650){
		    if(time1[T1]>500){
		      armMove(-45);
		      if(SensorValue[armPot]<=armLowScore+10) armMove(0);
		    }
		    driveArcade(-100,0,false,false);
		  }
		  armMove(0);
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//wait to repostition robot for 2 seconds
		  while(time1[T1]<2000){
		    if(SensorValue[button]==1)// lift arm to right height
		       armMove(35);
		    else
		       armMove(0);
		  }

		  if(offense){//notmal

		  time1[T1]=0;//Drive to the wall
		  spinMove(128);//because omnomnomnom
		  while(time1[T1]<3000){
		    driveArcade(40,0,false,false);
      }

		  spinMove(0);
      driveArcade(0,0,false,false);
		  time1[T1]=0;//Drive home
		  while(time1[T1]<1500){
		    driveArcade(-99,0,false,false);
		  }
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//wait to repostition robot for 2 seconds
		  while(time1[T1]<2000){
		  }

		  while(SensorValue[armPot]<=armLowScore){
		    armMove(40);
		}
		armMove(0);
		driveArcade(0,0,false,false);
	}//end offense
	else{ //not offense (anti-wallbot)
	  while(SensorValue[button]!=1)//button.waitforpress()
	    driveArcade(0,0,false,false);
	  time1[T1]=0;
	  while(time1[T1]<5000){
	    if(SensorValue[armPot]>armMidScore)
	      armMove(-50);
	    driveArcade(70,0,false,false);
	}
	driveArcade(0,0,false,false);
  }//end defense
	  }//end this color
  }//end interaction

 else //if in isolation
 {
   if (red || !red) //if on the red or blue team
   {
     int sign=1;//we must turn opposite directions for different colors.
     if(red) sign=1;
     else sign=-1;

     if(offense){
        time1[T1]=0;
        while(time1[T1]<1200){
          if(time1[T1] < 1200) //flip out rotors and intake pre-load
			      spinMove(128);
			    else spinMove(0);
			    if (time1[T1] < 500) //let's go up a bit before we start moving
			      armMove(70);

        }
     }//end offense
     else{//defense
     time1[T1] = 0;
		  while(time1[T1] < 1200) //flip out rotors and intake pre-load
			    spinMove(128);
			spinMove(0);

			time1[T1] = 0;
		  while (time1[T1] < 500) //let's go up a bit before we start moving
			  armMove(70);
      }//end defense


			time1[T1]=0;
		  offset=SensorValue[encoderLeft];
		  while ((SensorValue[encoderLeft]-offset)/392*revDistance < 25 && time1[T1]<6000) //Drive forwards to their middle goal and raise arm
		  {
		    if(SensorValue[armPot]<armMidScore+400) armMove(80);
		    else armMove(-5);
		    if((SensorValue[encoderLeft]-offset)/392*revDistance < 12)//move up a bit
		       driveArcade(60,0,false,false);
		    else /*if((SensorValue[encoderLeft]-offset)/392*revDistance < 30)*/{//turn to line up with goal
		      driveArcade(40,sign*-17,false,false);
		      //if((SensorValue[encoderLeft]-offset)/392*revDistance > 22)
		      //  spinMove(-40);
		    }
		  }
		  driveArcade(20,0,false,false);

		  time1[T1] = 0;
		  while (time1[T1] < 1500) //Release load and score in across goal!! (while holding up the arm)
			  spinMove(-80);
	/*		time1[T1] = 0;
		  while (time1[T1] < 350)
			  driveArcade(-30,0,false,false);
			time1[T1] = 0;
		  while (time1[T1] < 700) //Back up and score in our goal!! (while holding up the arm)
			  spinMove(-80);

	*/
		  armMove(0);
		  spinMove(0);

		  time1[T1]=0;
		  offset=SensorValue[encoderLeft];
		  while((SensorValue[encoderLeft]-offset)/392*revDistance > -25){//drive home and lower arm
		    if(time1[T1]>500){
		      armMove(-128);
		      if(SensorValue[armPot]<=armLowScore) armMove(0);
		    }
		    driveArcade(-80,sign*20,false,false);
		  }

		  driveArcade(0,0,false,false);

		  time1[T1]=0;
		  while(time1[T1]<5000){//keep going down if button press, wait to reposition
		    if(SensorValue[button]==1)
		      armMove(-0.4*(SensorValue[armPot]-armFloor+500));
		    else armMove(0);
		  }

		  spinMove(128);
		  armMove(-10);
		  time1[T1]=0;
		  offset=SensorValue[encoderLeft];
		  while((SensorValue[encoderLeft]-offset)/392*revDistance < 65 && time1[T1]<4000){//Drive diagonally through pieces
		    driveArcade(40,0,false,false);
      }

      driveArcade(0,0,false,false);
      spinMove(0);

      time1[T1]=0;//Come back home and lift arm
		  while(time1[T1]<2000){
		    driveArcade(-80,0,false,false);
		    armMove(90);
      }
      driveArcade(0,0,false,false);
      armMove(30);


      time1[T1]=0;//Drop in tall goal
		  while(time1[T1]<5000){
		    if(time1[T1]>2000)
		      spinMove(-128);
      }
      spinMove(0);

   }//end this color

 }//end isolation


}//end autonomous
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// User control code here, inside the loop

resetDriveVariables();
  armState = 0;
  bool isSeeking = false;
  int seekHeight = 0;
	  // This is the main execution loop for the user control program. Each time through the loop
	  // your program should update motor + servo values based on feedback from the joysticks.
  //turn(90,45,true);//this is a test
  //driveArcade(0,0,false,false);
	while (true)
	{
    driveArcade(vexRT[Ch3],vexRT[Ch4],true,true);//leave both square and ramp as false until the code is proven to work

    if (vexRT[Btn6U] != 0){
      spinInput = 128;
      slowSpin = false;
    }
      else if(vexRT[Btn6D] != 0)
      {
        spinInput = -128;
        slowSpin = false;
      }
      else if(vexRT[Btn5U] != 0)
      {
        spinInput = 40;
        slowSpin = true;
      }
      else if(vexRT[Btn5D]){
        spinInput = -31;
        slowSpin = true;
      }
      else
      {
        spinInput = 0;
        slowSpin = false;
      }


      if(vexRT[Btn8U])
      {
        isSeeking = true;
        seekHeight = armHighScore;
      }
      else if(vexRT[Btn8R])
      {
        isSeeking = true;
        seekHeight = armMidScore;
      }
      else if(vexRT[Btn8D])
      {
        isSeeking = true;
        seekHeight = armLowScore;
      }

      if(vexRT[Ch2]<-5||vexRT[Ch2]>5)
      {
        armMove(vexRT[Ch2]);
        isSeeking = false;
      }
      else if(isSeeking)
      {
        int error=seekHeight-SensorValue[armPot];
        if(error>30)
        {
          int power=error*.4;
          armMove(fmin(power,80));
        }
        else if(error<-30)
        {
          int power=error*.2;
          armMove(fmax(power,-60));
        }
        else
        {
          armMove(0);
          isSeeking = false;
        }
      }
      else
      {
        armMove(0);
      }
    spinMove(spinInput);

  }
}
